#ifndef MOTION_CONTROL_H
#define MOTION_CONTROL_H

#include "eigen3/Eigen/Eigen"
#include <vector>

using namespace std;
using namespace Eigen;

class motion_control
{
private:
    vector<Vector3d> path;    // 路径
    double foresee;           // 前瞻
    double speed;             // 速度
    double P;                 // 方向控制P系数
    double D;
    double errLast;
    double a_b;               //前瞻圆椭
    double tread;             // 左右轮距
    double tracking_flag;     // 标记是否已上轨道
    size_t last_aim_index;    // 上次控制时的目标点索引号
    bool isRotation;          //自转标志位
    double riseRotation;            //自转之后加速系数

    double max_acc;           // 最大加速度

    size_t nearest (Vector3d p);                      // 路径上最接近的点
    size_t next (size_t index);                       // 路径上下一点，最后一个点的下一点是第一点


public:
    motion_control(vector<Vector3d> path);    // 构造函数，输入路径

    void set_foresee (double foresee);        // 设置前瞻
    void set_speed (double speed);            // 设置速度
    void set_P (double P);                    // 设置方向控制P系数
    void set_D (double D);
    void set_a_b (double a_b);

    Vector3d aim_point (Vector3d pos, Vector3d dir);  // 寻找目标点
    vector<double> track (Vector3d pos, Vector3d dir);    // 计算两轮速度

    void getLidarData(vector<Vector3d> lidarList);  //获取激光雷达数据
};

/* 样例伪代码
 * path = read_path ();             // 获取路径
 * motion_control mc (path);        // 创建运动控制对象
 * mc.set_foresee (0.5);            // 设置前瞻
 * mc.set_speed (0.5);              // 设置速度
 * mc.set_P (1.5);                  // 设置方向控制P系数
 *
 * while (1) {                      // 主循环
 *     pos = get_pos();             // 获取机器人位置
 *     dir = get_dir();             // 获取机器人方向
 *     speed = mc.track (pos,dir);  // 计算两轮速度
 *     serial_output (speed);       // 串口发送速度控制指令
 * }
 */

#endif // MOTION_CONTROL_H
